/*
 * File: laser2tcp.cpp
 * Created On: 2020-04-11
 * Copyright © 2020 Bosch Rexroth AG
 */

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "std_msgs/String.h"
#include "msg2tcp/BinaryInterfaceServer.h"
#include "msg2tcp/LaserServer.h"
#include "tf/transform_datatypes.h"
#include "cmath"
// Open the socket

LaserServer::LaserServer(int port) : port_(port), laser_number_(0)
{
  server_ = new BinaryInterfaceServer(port);
}

/*void LaserServer::updateLaser(const LaserMessage simple_msg) {
  laser_number_++;
  LaserMessage data = simple_msg;
  data.scanNum = laser_number_;
  server_->write(&data, sizeof(data));
}*/

void LaserServer::updateLaser(const LaserMessage simple_msg)
{
  // laser_number_++;
  // LaserMessage data = simple_msg;
  // LaserMessage simple_msg;
  // data.scanNum = laser_number_;
  int buffer_size = sizeof(float) * (simple_msg.ranges.size() + simple_msg.intensities.size()) + 83;
  int index = 0;
  char *p = (char *)malloc(buffer_size);
  memcpy(p + index, &simple_msg.scanNum, 2);
  index += 2;
  memcpy(p + index, &simple_msg.time_start, 8);
  index += 8;
  memcpy(p + index, &simple_msg.uniqueId, 8);
  index += 8;
  memcpy(p + index, &simple_msg.duration_beam, 8);
  index += 8;
  memcpy(p + index, &simple_msg.duration_scan, 8);
  index += 8;
  memcpy(p + index, &simple_msg.duration_rotate, 8);
  index += 8;
  memcpy(p + index, &simple_msg.numBeams, 4);
  index += 4;
  memcpy(p + index, &simple_msg.angleStart, 4);
  index += 4;
  memcpy(p + index, &simple_msg.angleEnd, 4);
  index += 4;
  memcpy(p + index, &simple_msg.angleInc, 4);
  index += 4;
  memcpy(p + index, &simple_msg.minRange, 4);
  index += 4;
  memcpy(p + index, &simple_msg.maxRange, 4);
  index += 4;
  memcpy(p + index, &simple_msg.rangeArraySize, 4);
  index += 4;
  for (auto rang : simple_msg.ranges)
  {
    memcpy(p + index, &rang, 4);
    index += 4;
  }
  memcpy(p + index, &simple_msg.hasIntensities, 1);
  index += 1;
  memcpy(p + index, &simple_msg.minIntensities, 4);
  index += 4;
  memcpy(p + index, &simple_msg.maxIntensities, 4);
  index += 4;
  memcpy(p + index, &simple_msg.intensityArraySize, 4);
  index += 4;
  for (auto intensity : simple_msg.intensities)
  {
    memcpy(p + index, &intensity, 4);
    index += 4;
  }

  server_->write(p, buffer_size);
  free(p);
}

// Broadcast to all clients the laser data
uint16_t currentScanNum{0}; // Initialize scan number from 0
void scanCallback(const sensor_msgs::LaserScan::ConstPtr &msg, LaserServer *pToLaserServer)
{
  int inf_num = 0; // Count number of inf in the ranges
  int nan_num = 0; // Count number of nan in the ranges

  std::cout << "***********************************************" << std::endl;
  ROS_INFO("trigger");
  LaserMessage simple_msg;
  // const std::chrono::duration<double> unixTimePoint = std::chrono::system_clock::now() - std::chrono::system_clock::time_point ();
  // simple_msg.time_start = unixTimePoint.count();
  double unix_UTC = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
  simple_msg.scanNum = ++currentScanNum; // Accumulate scan number as ID of scans
  // simple_msg.scanNum = msg->seq;
  simple_msg.time_start = unix_UTC / 1000000.;
  // simple_msg.time_start = msg->header.stamp.toSec();
  // simple_msg.time_start = ros::Time::now();
  simple_msg.numBeams = msg->ranges.size(); // rangeSize get from ros message
  simple_msg.rangeArraySize = simple_msg.numBeams;
  simple_msg.intensityArraySize = simple_msg.numBeams;
  if ((msg->angle_min < -M_PI) || (msg->angle_max > M_PI)) // angleRange is limited to -pi to pi
  {
    simple_msg.angleStart = (0 - (msg->angle_max - msg->angle_min) / 2); // angleStart by rearrange
    simple_msg.angleEnd = (msg->angle_max - msg->angle_min) / 2;
  }
  else
  {
    simple_msg.angleStart = msg->angle_min; // angleStart get from ros message
    simple_msg.angleEnd = msg->angle_max;
  }
  simple_msg.angleInc = msg->angle_increment; // angleIncrement get from ros message
  // simple_msg.angleEnd = simple_msg.angleStart + msg->angle_increment * (simple_msg.numBeams - 1);
  // simple_msg.angleEnd = msg->angle_min + msg->angle_increment * (simple_msg.numBeams - 1);  //calculate angleEnd by equation
  simple_msg.minRange = msg->range_min; // constant
  simple_msg.maxRange = msg->range_max; // constant

  simple_msg.duration_beam = msg->time_increment;
  simple_msg.duration_scan = simple_msg.duration_beam * simple_msg.numBeams; // calculate scan time
  simple_msg.duration_rotate = simple_msg.duration_scan * ((simple_msg.angleEnd - simple_msg.angleStart) / (2 * M_PI));
  // if (msg->scan_time < 0.01) { simple_msg.duration_rotate = 1/20; }   //if time of one full rotate is not given, set documented frequency
  // simple_msg.duration_beam = simple_msg.duration_scan/simple_msg.numBeams; //calculate duration of each beam by dividing scan duration

  printf("scanNum: %d\n", simple_msg.scanNum);
  // check time
  // std::cout << simple_msg.time_start << std::endl;
  printf("Current time: %f\n", simple_msg.time_start);
  std::cout << "Start angle: " << simple_msg.angleStart << std::endl;
  std::cout << "Angle min: " << msg->angle_min << std::endl;
  std::cout << "End angle: " << simple_msg.angleEnd << std::endl;
  std::cout << "Angle max: " << msg->angle_max << std::endl;
  std::cout << "Number of beams: " << msg->ranges.size() << std::endl;
  printf("msg header time: %f\n", msg->header.stamp.toSec());
  // check frequency and number of beams
  std::cout << " One rotation time after change: " << simple_msg.duration_rotate << std::endl;
  std::cout << " Frequence(Hz): " << 1 / simple_msg.duration_rotate << std::endl;
  std::cout << " Number of Beams: " << simple_msg.numBeams << std::endl;
  for (size_t i = 0; i < simple_msg.numBeams; ++i)
  {
    /// simple_msg.ranges[i] = msg->ranges[i];
    // std::cout << typeid(msg->ranges[i]).name() << std::endl;
    if (msg->ranges[i] == +1.0 / 0.0)
    {
      inf_num = inf_num + 1;
      simple_msg.ranges.push_back(msg->range_max);
    }
    else if (msg->ranges[i] != msg->ranges[i])
    {
      nan_num = nan_num + 1;
      simple_msg.ranges.push_back(msg->range_min);
    } /*
     else if(msg->ranges[i] > 100)
     {
       simple_msg.ranges.push_back(msg->range_max);
     }*/
    else
    {
      //  std::cout << "the range is not infinite:  " << msg->ranges[i] << std::endl;
      simple_msg.ranges.push_back(msg->ranges[i]); // if it's inf, send max value instead
    }

    // simple_msg.ranges.push_back(msg->ranges[i]);
    // std::cout << msg->ranges[i] << std::endl;
  }

  simple_msg.hasIntensities = true;

  for (float intensity : msg->intensities)
  {
    simple_msg.intensities.push_back(intensity);
  }

  std::cout << "Note: Num of inf is " << inf_num << " , Num of nan is " << nan_num << std::endl;
  std::cout << "Set all Inf to range_max and Nan to range_min ." << std::endl;
  pToLaserServer->updateLaser(simple_msg);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "laser2tcp_node");
  ros::NodeHandle n("~");

  int port = 2112;
  std::string laser_topic = "/scan";
  // std_msgs::String ip;

  if (n.getParam("port", port))
  {
    ROS_INFO("Got 'port' param: %d", port);
  }
  else
  {
    ROS_ERROR("Failed to get param 'port'");
  }

  if (n.getParam("laser_topic", laser_topic))
  {
    ROS_INFO("Got 'laser_topic' param: %s", laser_topic.c_str());
  }
  else
  {
    ROS_ERROR("Failed to get param 'laser_topic'");
  }

  // LaserServer* odom_server_ptr = newOdome(port);
  LaserServer *pToLaserServer = new LaserServer(port);
  // odom_server = new LaserServer(port);

  // Subscribe to laser scan topic
  ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>(laser_topic, 1, boost::bind(&scanCallback, _1, pToLaserServer));
  // Open socket of choice

  ros::spin();

  return 0;
}
